// newton.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include "common.h"
#include "math.h"
#include "iostream.h"

node Node[NODE_NUM];
pkt Packet[NODE_NUM];

int main(int argc, char* argv[])
{
	int i,j,k,nbrIP,StepsizeIters,NewtonIters,DualIters,stopflag;
	float tmp;
	//configure the power grid
    nodeConfigure(Node);

	//communication......
	for (i=0; i<NODE_NUM; i++) {
		initPkt(Node[i], Packet[i]);
	}
	for (i=0; i<NODE_NUM; i++) {
		parseInitPkt(Node[i], Packet);
	}

	//each node initilize according to the received messages
	for (i=0; i<NODE_NUM; i++) {
		nodeInit(Node[i]);
	}

	///////////////////start to newton computation///////////////////////
	//initilize variables
	for (i=0; i<NODE_NUM; i++) {
		initVariables(Node[i]);
	}

	//communication.....dual variable and voltage
	for (i=0; i<NODE_NUM; i++) {
		Packet[i].dualvar = Node[i].dualvar;
		for (j=0; j<Node[i].oneHopNbr_nums; j++) {
			Packet[i].oneHopNbr_flow[j] = Node[i].oneHopNbr_flow[j];
			Packet[i].oneHopNbr_IP[j] = Node[i].oneHopNbr_IP[j];
		}
	}
	for (i=0; i<NODE_NUM; i++) {
		for (j=0; j<Node[i].oneHopNbr_nums; j++) {
			nbrIP = Node[i].oneHopNbr_IP[j];
			Node[i].oneHopNbr_dualvar[j] = Packet[nbrIP].dualvar;
			for (k=0; k<Node[i].oneHopNbr_nbrnums[j]; k++){
				if (Packet[nbrIP].oneHopNbr_IP[k] == Node[i].ipaddress) {
					Node[i].oneHopNbr_flow[j] = Packet[nbrIP].oneHopNbr_flow[k]; 
					break;
				}
			}
		}
	}
	
	//initilize the form of residual function
	for (i=0; i<NODE_NUM; i++) {
		initResForm(Node[i]);
	}


	//
	for (StepsizeIters=0; StepsizeIters<50; StepsizeIters++){
		//communication.....current form of residual function
		for (i=0; i<NODE_NUM; i++) {
			Packet[i].form = Node[i].form;
		}
	    for (i=0; i<NODE_NUM; i++) {
			for (j=0; j<Node[i].oneHopNbr_nums; j++) {
				nbrIP = Node[i].oneHopNbr_IP[j];
				Node[i].oneHopNbr_form[j] = Packet[nbrIP].form;
			}
		}
		
		//update the form of residual function
		for (i=0; i<NODE_NUM; i++){
             Node[i].form *= Node[i].form_weight;
             for (j=0; j<Node[i].oneHopNbr_nums; j++){
                 Node[i].form += Node[i].oneHopNbr_form[j]*Node[i].oneHopNbr_form_weight[j];
             }  
         }
	}

	//cash the form of residual function
	for (i=0; i<NODE_NUM; i++){
         Node[i].form_previous = sqrt(Node[i].form*NODE_NUM);
    } 


	//newton iterations
	for (NewtonIters=0; NewtonIters<100; NewtonIters++){
		for (i=0; i<NODE_NUM; i++) {
			Node[i].stepsize = 0;
		}

		//update the first-order and second-order gradient
		for (i=0; i<NODE_NUM; i++) {
			updateGradient(Node[i]);
			updateHassion(Node[i]);
		}
		
		////;;;;;;;
		for (i=0; i<NODE_NUM; i++)
		{
			cout<<Node[i].generator_gradient[0]<<endl;
		}
		for (i=0; i<NODE_NUM; i++)
		{
			cout<<Node[i].demand_gradient[0]<<endl;
		}
		for (i=0; i<NODE_NUM; i++)
		{
			for (j=0; j<Node[i].oneHopNbr_nums; j++) {
				if (Node[i].oneHopNbr_flow_direction[j] == 1) {
					cout<<Node[i].oneHopNbr_flow_gradient[j]<<endl;
				}
			}
			
		}

		cout<<endl;
		for (i=0; i<NODE_NUM; i++)
		{
			cout<<Node[i].generator_hassion[0]<<endl;
		}
		for (i=0; i<NODE_NUM; i++)
		{
			cout<<Node[i].demand_hassion[0]<<endl;
		}
		for (i=0; i<NODE_NUM; i++)
		{
			for (j=0; j<Node[i].oneHopNbr_nums; j++) {
				if (Node[i].oneHopNbr_flow_direction[j] == 1) {
					cout<<Node[i].oneHopNbr_flow_hassion[j]<<endl;
				}
			}
			
		}
		cout<<endl;
			
		//communication.....the values of the first-order and second-order gradient
		for (i=0; i<NODE_NUM; i++) {
			Packet[i].dualvar = Node[i].dualvar;
			for (j=0; j<Node[i].oneHopNbr_nums; j++) {
				Packet[i].oneHopNbr_flow[j] = Node[i].oneHopNbr_flow[j];
				Packet[i].oneHopNbr_flow_gradient[j] = Node[i].oneHopNbr_flow_gradient[j];
				Packet[i].oneHopNbr_flow_hassion[j] = Node[i].oneHopNbr_flow_hassion[j];
				Packet[i].oneHopNbr_IP[j] = Node[i].oneHopNbr_IP[j];
			}
		}
		for (i=0; i<NODE_NUM; i++) {
			for (j=0; j<Node[i].oneHopNbr_nums; j++) {
				nbrIP = Node[i].oneHopNbr_IP[j];
				Node[i].oneHopNbr_dualvar[j] = Packet[nbrIP].dualvar;
				for (k=0; k<Node[i].oneHopNbr_nbrnums[j]; k++){
					if (Packet[nbrIP].oneHopNbr_IP[k] == Node[i].ipaddress && Node[i].oneHopNbr_flow_direction[j] == -1) {
						Node[i].oneHopNbr_flow[j] = Packet[nbrIP].oneHopNbr_flow[k]; 
						Node[i].oneHopNbr_flow_gradient[j] = Packet[nbrIP].oneHopNbr_flow_gradient[k];
						Node[i].oneHopNbr_flow_hassion[j] = Packet[nbrIP].oneHopNbr_flow_hassion[k];
						break;
					}
				}
			}
		}

		//update the weight that used to estimate the dual variables and AX-AHf
		for (i=0; i<NODE_NUM; i++) {
			updateDualWeight(Node[i]);
			updateDualobservation(Node[i]);
			Node[i].dualvar_delta = 0;
		}

		for (i=0; i<NODE_NUM; i++){
			cout<<Node[i].dualvar_observation<<endl;
		}

		//estimate the dual variables
		for (DualIters=0; DualIters<100; DualIters++){ 
			//communication......dual variables.....one hop and two hop neighbors
			for (i=0; i<NODE_NUM; i++) {
				Packet[i].dualvar_delta = Node[i].dualvar_delta;
			}
			for (i=0; i<NODE_NUM; i++) {
				//one hop
				for (j=0; j<Node[i].oneHopNbr_nums; j++) {
					nbrIP = Node[i].oneHopNbr_IP[j];
					Node[i].oneHopNbr_dualvar_delta[j] = Packet[nbrIP].dualvar_delta;
				}
			}

			//update the dual variable
			for (i=0; i<NODE_NUM; i++) {
				Node[i].dualvar_delta *= Node[i].dualvar_weight;
				for (j=0; j<Node[i].oneHopNbr_nums; j++) {
					Node[i].dualvar_delta += Node[i].oneHopNbr_dualvar_delta[j]*Node[i].oneHopNbr_dualvar_weight[j];
				}

				Node[i].dualvar_delta -= Node[i].dualvar_observation;
				Node[i].dualvar_delta *= -pow(Node[i].dualvar_factor,-1);
				cout<<Node[i].dualvar_delta<<endl;
			}
		}

		for (i=0; i<NODE_NUM; i++) {
			cout<<Node[i].dualvar_delta<<endl;
		}

		
		//communication.....the estimated dual variable
		for (i=0; i<NODE_NUM; i++) {
			Packet[i].dualvar_delta = Node[i].dualvar_delta;
			//cout<<Node[i].dualvar<<endl;
		}
		for (i=0; i<NODE_NUM; i++) {
			for (j=0; j<Node[i].oneHopNbr_nums; j++) {
				nbrIP = Node[i].oneHopNbr_IP[j];
				Node[i].oneHopNbr_dualvar_delta[j] = Packet[nbrIP].dualvar_delta;
			}
		}

		//update dx
		for (i=0; i<NODE_NUM; i++) {
			updatePrimalDelta(Node[i]);
		}

		cout<<endl;
		for (i=0; i<NODE_NUM; i++)
		{
			cout<<Node[i].generator_delta[0]<<endl;
		}
		for (i=0; i<NODE_NUM; i++)
		{
			cout<<Node[i].demand_delta[0]<<endl;
		}
		for (i=0; i<NODE_NUM; i++)
		{
			for (j=0; j<Node[i].oneHopNbr_nums; j++) {
				if (Node[i].oneHopNbr_flow_direction[j] == 1) {
					cout<<Node[i].oneHopNbr_flow_delta[j]<<endl;
				}
			}
			
		}

		//communication......newton direction of voltage
		for (i=0; i<NODE_NUM; i++) {
			Packet[i].dualvar = Node[i].dualvar;
			for (j=0; j<Node[i].oneHopNbr_nums; j++) {
				Packet[i].oneHopNbr_flow[j] = Node[i].oneHopNbr_flow[j];
				Packet[i].oneHopNbr_IP[j] = Node[i].oneHopNbr_IP[j];
			}
		}
		for (i=0; i<NODE_NUM; i++) {
			for (j=0; j<Node[i].oneHopNbr_nums; j++) {
				nbrIP = Node[i].oneHopNbr_IP[j];
				Node[i].oneHopNbr_dualvar[j] = Packet[nbrIP].dualvar;
				for (k=0; k<Node[i].oneHopNbr_nbrnums[j]; k++){
					if (Packet[nbrIP].oneHopNbr_IP[k] == Node[i].ipaddress  && Node[i].oneHopNbr_flow_direction[j] == -1) {
						Node[i].oneHopNbr_flow[j] = Packet[nbrIP].oneHopNbr_flow[k]; 
						break;
					}
				}
			}
		}
		
		/////estimate the stepsize
		for (i=0; i<NODE_NUM; i++) {
			Node[i].stepsize = 1;
        }

		stopflag =0 ;
		while (1) {
			for (i=0; i<NODE_NUM; i++) {
				initResForm(Node[i]);
			}
	//
			for (StepsizeIters=0; StepsizeIters<50; StepsizeIters++){
		//communication.....current form of residual function
				for (i=0; i<NODE_NUM; i++) {
					Packet[i].form = Node[i].form;
				}
				 for (i=0; i<NODE_NUM; i++) {
					for (j=0; j<Node[i].oneHopNbr_nums; j++) {
						nbrIP = Node[i].oneHopNbr_IP[j];
						Node[i].oneHopNbr_form[j] = Packet[nbrIP].form;
					}
				}
		
				//update the form of residual function
				for (i=0; i<NODE_NUM; i++){
					Node[i].form *= Node[i].form_weight;
					for (j=0; j<Node[i].oneHopNbr_nums; j++){
						Node[i].form += Node[i].oneHopNbr_form[j]*Node[i].oneHopNbr_form_weight[j];
					}  
				}
			}

				//cash the form of residual function
			for (i=0; i<NODE_NUM; i++){
				tmp = sqrt(Node[i].form*NODE_NUM);
				Node[i].form = tmp;
			}

			for (i=0; i<NODE_NUM; i++){
				if (Node[i].form>(1-Node[i].stepsize*CONSTANT_STEPSIZE_ALPHA)*Node[i].form_previous+CONSTANT_ETA) {
					Node[i].stepsize *= CONSTANT_STEPSIZE_BETA;
				}
				else {
					stopflag = 1;
					break;
				}
			}

			if (stopflag == 1) 
				break;
		}

		//update primal and cash the stepsize
		for (i=0; i<NODE_NUM; i++) {
			Node[i].form_previous = Node[i].form;
			updatePrimalVal(Node[i]);
			updateDualVal(Node[i]);
		}

		cout<<endl;
		for (i=0; i<NODE_NUM; i++)
		{
			cout<<Node[i].generator[0]<<endl;
		}
		for (i=0; i<NODE_NUM; i++)
		{
			cout<<Node[i].demand[0]<<endl;
		}
		for (i=0; i<NODE_NUM; i++)
		{
			for (j=0; j<Node[i].oneHopNbr_nums; j++) {
				if (Node[i].oneHopNbr_flow_direction[j] == 1) {
					cout<<Node[i].oneHopNbr_flow[j]<<endl;
				}
			}
			
		}


	    cout<<endl;
		for (i=0; i<NODE_NUM; i++) {
			cout<<Node[i].dualvar_delta<<endl;
		}

	}
	return 0;
}


